import lejos.nxt.*;
import lejos.robotics.proposal.DifferentialPilot;
import lejos.robotics.subsumption.*;

public class Main {
	public static void main(String [] args) throws InterruptedException{
		//TODO: waarden correct invullen
		final float leftWheelDiameter = 56; 
		final float rightWheelDiameter = 56;
		final float trackWidth = 110;
		Motor leftMotor = Motor.B;
		Motor rightMotor = Motor.C;
		
		DifferentialPilot pilot = new DifferentialPilot(leftWheelDiameter, rightWheelDiameter, trackWidth, leftMotor, rightMotor, false);
		LightSensor colorLightsensor = new LightSensor(SensorPort.S1);
		//UltrasonicSensor ultrasonicSensor = new UltrasonicSensor(SensorPort.S2);
		
		LCD.drawString("ClassDef", 0, 0);
		colorLightsensor.calibrateHigh();
		LCD.drawString("Calibrate high", 0, 0);
		Thread.sleep(5000);
		colorLightsensor.calibrateLow();
		LCD.drawString("Calibrate low", 0, 0);
		Thread.sleep(5000);
		
		while (true) {
		      LCD.drawInt(colorLightsensor.getLightValue(), 4, 0, 0);
		      LCD.drawInt(colorLightsensor.getNormalizedLightValue(), 4, 0, 1);
		      LCD.drawInt(SensorPort.S1.readRawValue(), 4, 0, 2);
		      LCD.drawInt(SensorPort.S1.readValue(), 4, 0, 3);
		}

		/*Behavior b1 = new DriveForward(pilot);
		Behavior b2 = new LookForLine(colorLightsensor, pilot);
		//afstand tot muur:10 cm, tolerantie van 1 cm
		Behavior b3 = new CorrectToParallel(10, 1, pilot, ultrasonicSensor);
		Behavior [] bArray = {b1, b2}; 
		Arbitrator arby = new Arbitrator(bArray);
		arby.start();*/
	}
}